
#ifndef __COMM_CAR_H__
#define __COMM_CAR_H__

#include "comm/comm_car_def.h"
#include "comm/comm_geometry.h"
#include "comm/comm_base.h"
#include <iostream>
#include <list>
#include <vector>

using namespace std;

class LASER_C
{
public:
    LASER_C(void);
    ~LASER_C(void);

    void SetPos(COORDINATE_2D_STRU *pstLaserPos, float laserDirAngle, float dirAngleSin, float dirAngleCos);
    void SetPose(const COOR_2D_POSE_STRU &stPose);
    const COORDINATE_2D_STRU &GetPos(void) const;
    const COOR_2D_POSE_STRU &GetPoseByMap(void);
    void GetPoseByMap(COOR_2D_POSE_STRU &stPose);

    void TransPosByMapToByLaser(const COORDINATE_2D_STRU &stPosByMap, COORDINATE_2D_STRU &stPosByLaser);

    void  InitLeftAvoidDist(float carMinTrunRadius, float laserXByCar, float protectXByCar, float protectYByCar);
    float GetLeftAvoidMinDist(unsigned int angleIdx);
    float GetRightAvoidMinDist(unsigned int angleIdx);

    float  GetPointAngleByIdx(int pointIdx) const;
    UINT32 GetPointAngleIdx(float angle) const;
    void   GetPointPos(float pointAngle, float pointDist, COORDINATE_2D_STRU *pstPointPos) const;
    void   GetPointPosByMap(UINT32 pointIdx, float pointDist, COORDINATE_2D_STRU &pointPosByMap) const;
    float  GetPointAngleByPos(const COORDINATE_2D_STRU &stPointPos) const;
    float  GetPointAngleByMapByIdx(int pointIdx) const;
    void   GetPointByPos(const COORDINATE_2D_STRU &stPointPos, LASER_POINT_STRU &stPoint) const;
    void   GetPointPosByLaser(unsigned int angleIdx, float dist, COORDINATE_2D_STRU *pstRelDist) const;
    void   GetPointPosByLaser(float angle, float dist, COORDINATE_2D_STRU *pstRelDist) const;

    void  SetBlindObstDetectDist(float distMin, float distMax);
    bool  GetRightBlindObst(const float aLaserPoint[], UINT32 laserPointNum, COORDINATE_2D_STRU *pstObstPos) const;
    bool  GetLeftBlindObst(const float aLaserPoint[], UINT32 laserPointNum, COORDINATE_2D_STRU *pstObstPos) const;

private:
    /* 从最小的负角向正角方向扫描 */
    float scanAgnleRange_;
    unsigned int scanPointNum_;
    float pointAngleBase_;
    float pointAngleInc_;                       /* 固定值 *//* 扫描点角度差 */
    float pointDensity_;                        /* 固定值 *//* 扫描点密集度，即为扫描点角度差得倒数 */

    /* 左右侧向测量盲区障碍物 */
    float blindObstDetectDistMin_;
    float blindObstDetectDistMax_;
    int   minRightBlindPointIdx_;            /* 固定 *//* 右侧盲区障碍识别最小激光点序号 */
    int   maxRightBlindPointIdx_;            /* 固定 *//* 右侧盲区障碍识别最大激光点序号 */
    int   minLeftBlindPointIdx_;             /* 固定 *//* 左侧盲区障碍识别最小激光点序号 */
    int   MaxLeftBlindPointIdx_;             /* 固定 *//* 左侧盲区障碍识别最大激光点序号 */


    vector<float> laserPointCos_;
    vector<float> laserPointSin_;
    vector<float> leftAvoidMinDistPerPoint_;     /* 小车从左边绕行最小距离 */

    COOR_2D_POSE_STRU stPoseByMap_;              /* 雷达的位姿 */

    COOR_TF_C objMapToLaserTf_;

    bool  GetBlindObst(const float aLaserPoint[], int minDetectIdx, int maxDetectIdx, COORDINATE_2D_STRU *pstObstPos) const;
};

typedef struct {
    float wheelBaseLen;                 /* 前后轴距 */
    float headToBackAxisLen;            /* 车头到后轴的距离 */
    float tailToBackAxisLen;            /* 车尾到后轴的距离 */
    float bodyWidth;                    /* 车身宽度 */
    float maxSteeringAngleAbs;          /* 最大转向角度 */
    float maxMoveVelAbs;                /* 最大行驶速度 */
} CAR_BODY_PARAM_STRU;

/**************************************************************************************
功能描述: 车身
修改记录:
**************************************************************************************/
class CAR_BODY_C
{
public:
    CAR_BODY_C(void)
    {
        stBodyParam_.wheelBaseLen        = CAR_DEF_WHEELBASE_LEN;
        stBodyParam_.headToBackAxisLen   = CAR_DEF_WHEELBASE_LEN + CAR_DEF_HEAD_TO_FRONT_WHEEL_LEN;
        stBodyParam_.tailToBackAxisLen   = CAR_DEF_TAIL_TO_BACK_WHEEL_LEN;
        stBodyParam_.bodyWidth           = CAR_DEF_BODY_WIDTH;
        stBodyParam_.maxSteeringAngleAbs = CAR_DEF_MAX_TURN_ANGLE_ABS;
        stBodyParam_.maxMoveVelAbs       = CAR_DEF_MAX_VEL;

        INT32 rslt = ResetBody(stBodyParam_);
        R_ASSERT(rslt == 0, NOTHING);
    }

    virtual ~CAR_BODY_C(void)
    {
    }

    void   GetLeftHeadPosByCarWithProtect(float headProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const;
    void   GetRightHeadPosByCarWithProtect(float headProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const;
    void   GetLeftBackAxisPosByCarWithProtect(float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const;
    void   GetRightBackAxisPosByCarWithProtect(float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const;
    void   GetLeftTailPosByCarWithProtect(float tailProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const;
    void   GetRightTailPosByCarWithProtect(float tailProtectWidth, float sideProtectWidth, COORDINATE_2D_STRU &stPosByCar) const;

    void   GetMaxAngleLeftTurnCenterPosByCar(COORDINATE_2D_STRU &stPosByCar) const;
    void   GetMaxAngleRightTurnCenterPosByCar(COORDINATE_2D_STRU &stPosByCar) const;
    void   GetLeftTurnCenterPosByCar(float steeringAngleAbs, COORDINATE_2D_STRU &stPosByCar) const;
    void   GetRightTurnCenterPosByCar(float steeringAngleAbs, COORDINATE_2D_STRU &stPosByCar) const;

    float  GetHeadHalfBodyLen(void) const;
    float  GetBackHalfBodyLen(void) const;
    float  GetBodyLenBeforeFrontWheel(void) const;
    float  GetBodyWidth(void) const;
    float  GetMaxSteeringAngleAbs(void) const;
    float  GetMaxMoveVelAbs(void) const;
    float  GetTurnRadius(float steeringAngle) const;
    float  GetSteeringAngle(float turnRadius) const;

protected:
    CAR_BODY_PARAM_STRU stBodyParam_;   /* 车体物理参数 */

    COOR_2D_POSE_STRU   stFrontWheelPoseByCar_;
    COORDINATE_2D_STRU  stLeftHeadPosByCar_;
    COORDINATE_2D_STRU  stRightHeadPosByCar_;
    COORDINATE_2D_STRU  stLeftTailPosByCar_;
    COORDINATE_2D_STRU  stRightTailPosByCar_;
    COORDINATE_2D_STRU  stLeftBackAxisPosByCar_;
    COORDINATE_2D_STRU  stRightBackAxisPosByCar_;
    COORDINATE_2D_STRU  stMaxAngleLeftTurnCenterPosByCar_;
    COORDINATE_2D_STRU  stMaxAngleRightTurnCenterPosByCar_;

    INT32 ResetBody(CAR_BODY_PARAM_STRU &stBodyParam);

    float GetWheelBaseLen(void) const;

private:
};

/**************************************************************************************
功能描述: 动态车辆
修改记录:
**************************************************************************************/
class DYNC_CAR_C : public CAR_BODY_C
{
public:
    DYNC_CAR_C(void)
    {
    }

    virtual ~DYNC_CAR_C(void)
    {
    }

    void  SetCarPose(const COOR_2D_POSE_STRU &stCarPoseByMap);
    void  SetFrontWheelPose(const COOR_2D_POSE_STRU &stPoseByMap);

    const COORDINATE_2D_STRU &GetCarPos(void) const;
    void  GetCarPose(COOR_2D_POSE_STRU &stCarPoseByMap) const;
    const COOR_2D_POSE_STRU &GetCarPose(void) const;
    const COORDINATE_2D_STRU &GetFrontWheelPos(void) const;
    void  GetFrontWheelPose(COOR_2D_POSE_STRU &stPoseByMap) const;
    const COOR_2D_POSE_STRU &GetFrontWheelPose(void) const;

    void  Get4CornerPosByMap(COORDINATE_2D_STRU &stLeftHeadPosByMap, COORDINATE_2D_STRU &stRightHeadPosByMap, COORDINATE_2D_STRU &stLeftTailPosByMap, COORDINATE_2D_STRU &stRightTailPosByMap, float sideProtectLen = 0.0, float faceProectLen = 0.0) const;

    void  TransformPosByMapToByCar(const COORDINATE_2D_STRU &stPosByMap, COORDINATE_2D_STRU &stPosByCar) const;
    void  TransformPosByCarToByMap(const COORDINATE_2D_STRU &stPosByCar, COORDINATE_2D_STRU &stPosByMap) const;

protected:
    COOR_2D_POSE_STRU stCarPoseByMap_;
    COOR_2D_POSE_STRU stCarFrontWheelPoseByMap_;

private:
    COOR_TF_C         objMapToCarTf_;
};

typedef struct {
    float steeringAngle;       /* 转向角 */
    float moveLinearVel;       /* 线速度 */
    float dirAngularVel;       /* 角速度 */
} CAR_MOVE_INFO_STRU;

/**************************************************************************************
功能描述: 移动车辆
修改记录:
**************************************************************************************/
class MOVE_CAR_C : public DYNC_CAR_C
{
public:
    MOVE_CAR_C(float timePerMove = 0.05, float maxAccAbs = 1.0e10)
    {
        timeLenPerMove_           = timePerMove;
        maxAccAbs_                = maxAccAbs;
        maxVelDiffOfPerPeriodAbs_ = maxAccAbs_ * timeLenPerMove_;
        inverseOfTimeLenPerMove_  = 1.0 / timePerMove;
        inverseOfWheelBaseLen_    = 1.0 / GetWheelBaseLen();

        ResetMoveInfo();
    }

    virtual ~MOVE_CAR_C (void)
    {
    }

    float Move(float steeringAngle, float linearVel);
    float MoveWithAccLimit(float steeringAngle, float linearVel);

    void  ResetMoveInfo(void);

    void  GetMoveInfo(COOR_2D_POSE_STRU &stCarPoseByMap, CAR_MOVE_INFO_STRU &stMoveInfo) const;

private:
    float               timeLenPerMove_;
    float               maxAccAbs_;
    float               maxVelDiffOfPerPeriodAbs_;
    float               inverseOfTimeLenPerMove_;
    float               inverseOfWheelBaseLen_;

    CAR_MOVE_INFO_STRU  stMoveInfo_;     /* 最新运动信息 */

    float GetLinearVelWithAccLimit(float linearVelCmd) const;
};

/**************************************************************************************
功能描述: 车辆驾驶器
修改记录:
**************************************************************************************/
class CAR_DRIVER_C
{
public:
    CAR_DRIVER_C(DYNC_CAR_C &objCar, BOOL isGoForward = true, BOOL isUseFrontWheelToGoForwardTrc = true)
                    : objCar_(objCar),
                      isGoForward_(isGoForward),
                      isUseFrontWheelToGoForwardTrc_(isUseFrontWheelToGoForwardTrc)
    {
    }

    virtual ~CAR_DRIVER_C(void)
    {
    }

    void  SetMoveDir(BOOL isGoForward, BOOL isUseFrontWheelToGoForwardTrc);
    BOOL  IsGoForward(void) const;

    void  SetRoutePose(const COOR_2D_POSE_STRU &stCarRoutePose);
    void  SetRoutePose(const POSE_2D_C &objRoutePose);
    void  GetRoutePose(COOR_2D_POSE_STRU &stCarRoutePose) const;
    void  GetRoutePose(POSE_2D_C &objRoutePose) const;

    float GetSteeringAngleOfFrontWheelTrc(const COORDINATE_2D_STRU &stAimPosByMap) const;
    float GetSteeringAngleOfBackWheelTrc(const COORDINATE_2D_STRU &stAimPosByMap) const;
    float GetAimPointSteeringAngle(const COORDINATE_2D_STRU &stAimPosByMap) const;

    float GetSimulationVel(void) const;
    float GetMaxMoveVel(void) const;

private:
    DYNC_CAR_C &objCar_;

    BOOL        isGoForward_;
    BOOL        isUseFrontWheelToGoForwardTrc_;
};


#endif

